%% Li Z, He B, Wang M, et al. Time-coordination entry guidance for multi-hypersonic vehicles ...
%                                        [J]. Aerospace Science and Technology, 2019, 89: 123-135.
clc
clear
close all
global m S d2r R0 g0 rot Vs ts Vup Vdown alphamax alphamin alphaLD dAziUp dAziDown ...
    epsilon dtInt dtGui dtIntPred dtGuiPred e0 ef absBankMax
d2r = pi/180;
m = 907;
S = 0.48;
R0 = 6378135;
g0 = 9.81;
Vs = sqrt(g0*R0);
ts = sqrt(R0/g0);
rot = 7.292115E-5;
epsilon = 1e-3; % newton iteration tolerance

lon0 = 6.5*d2r;
lat0 = 7.5*d2r;
Vf = 2000/Vs;
tf = 1440/ts;
fpa0 = -1*d2r;
azi0 = 31*d2r;
h0 = 72e3;
rad0 = (R0 + h0)/R0;
V0 = 6100/Vs;
e0 = 1/rad0 - V0^2/2;
lonf = 50*d2r;
latf = 50*d2r;
hf = 30e3;
radf = (R0 + hf)/R0;
ef = 1/radf - Vf^2/2;
alphamax = 20*d2r;
alphaLD = 12*d2r;
alphamin = 7*d2r;
absBankMax = 60*d2r;
Vup = 5900/Vs;
Vdown = 5000/Vs;
% 航向角误差走廊宽度
dAziUp = 10*d2r;
dAziDown = -10*d2r;
dtInt = 0.1/ts; % 积分周期
dtGui = 1/ts; % 制导周期
dtIntPred = 1/ts;
dtGuiPred = 10/ts;
p = 1;
state(p,:) = [rad0, lon0, lat0, V0, fpa0, azi0];
statef = [radf, lonf, latf, Vf, nan, nan];
time(p,1) = 0;
ctrl(p,:)= [20*d2r, 0, 1]; % 攻角，倾侧角幅值，倾侧角符号
auxState(p,:) = [40*d2r, 30*d2r, 0]; % absBank absBankf iter
k = dtGui/dtInt;
fpaflag = false(1);
while 1
    state_temp = RK4(@ReentryDynamic, state(p,:), ctrl(p,:), dtInt);
    if k <= 0
        if fpaflag
            [ctrl_temp, aux] = GuidanceOutLoop(time(p,1), tf, state(p,:), statef, ctrl(p,:), auxState(p,:));
        else
            ctrl_temp = GuidanceDes();
        end
        k = dtGui/dtInt;
    else 
        ctrl_temp = ctrl(p,:);
    end
    ctrl(p+1,:) = ctrl_temp;
    state(p+1,:) = state_temp;
    time(p+1,1) = time(p,1) + dtInt;
    r = state_temp(1);
    lon = state_temp(2);
    lat = state_temp(3);
    V = state_temp(4);
    fpa = state_temp(5);
    if fpa > 0
        fpaflag = true(1);
    end
    e = 1/r - V^2/2;
    auxState(p+1,:) = auxState(p,:);
    if exist('aux', 'var')
        auxState(p+1, :) = aux;
    end
    p = p+1;
    k = k-1;
    if e >= ef || (lonf-lon)^2 + (latf-lat)^2 < 2*(0.1*d2r)^2
        break;
    end
end
%% plot
time = ts*time;
alt = (state(:, 1) - 1)*R0;
lon = state(:, 2)*180/pi;
lat = state(:, 3)*180/pi;
V = state(:, 4)*Vs;
fpa = state(:, 5)*180/pi;
azi = state(:, 6)*180/pi;
alpha = ctrl(:, 1)*180/pi;
absBank = ctrl(:, 2)*180/pi;
signOfBank = ctrl(:, 3);
bank = absBank.*signOfBank;
absBank0 = auxState(:, 1)*180/pi;
absBankmid = auxState(:, 2)*180/pi;
iter = auxState(:, 3);

figure
plot3(lon, lat, alt, 'linewidth', 2);
xlabel('Longitude');ylabel('Latitude');zlabel('Altitude (km)');
grid on

figure
pp = plot(time, alt, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Altitude (km)');

figure
plot(lon, lat, 'linewidth', 1.5);
xlabel('Longitude (deg)'); ylabel('Latitude (deg)');

figure
plot(time, V, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Speed (km/s)');

figure
plot(time, fpa, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Flight Path Angle (deg)');

figure
plot(time, azi, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Azimuth Angle (deg)');

figure
plot(time, alpha, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Angle of Attack (deg)');

figure
plot(time, bank, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Bank Angle (deg)');

figure
plot(time, signOfBank, 'o');
xlabel('Time (s)'); ylabel('Sign Of Bank Angle (deg)');

figure
plot(time, absBank0, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('absBank0 (deg)');

figure
plot(time, absBankmid, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('absBankmid (deg)');

figure
plot(time, iter, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('iterations (deg)');